#include "control_task.h"	
#include "SCSProtocol.h"
#include "sensor_task.h"

#include "mpu_module.h"
#include "usbd_cdc_if.h"
#include "cmsis_os.h"
#include "usb_device.h"
#include "scs_uart.h"
#include "usart.h"
#include "arm_math.h"
long euler[10];
float32_t rot_mat[10];
#define pitch body.pitch
#define roll 	body.roll
#define yaw 	body.yaw

int32_t torque[14];

void MPU_Task(void const * argument)
{
  /* USER CODE BEGIN MPU_Task */
  /* Infinite loop */
  for(;;)
  {
    mpu_module_sampling();

    mpu_read_euler(euler);
    mpu_read_rot_mat(rot_mat);
    arm_matrix_instance_f32 pSrcA, pSrcB, pDest; //声明矩阵
    float32_t feet_vec[3] = {0, 0, 1}, dest[3];//zxy
    arm_mat_init_f32(&pSrcA, 3, 3, rot_mat); //初始化矩阵
    arm_mat_init_f32(&pSrcB, 3, 1, feet_vec); //初始化矩阵
    arm_mat_init_f32(&pDest, 3, 1, dest); //初始化矩阵
    arm_mat_mult_f32(&pSrcA, &pSrcB, &pDest); //矩阵相乘 一次需要3*3矩阵需要3.2us
		float32_t len = sqrt(dest[0]*dest[0] + dest[1]*dest[1] + dest[2]*dest[2]);
		for(char i = 0; i < 3; i++) dest[i] /= len;
		for(char i = 0; i < 3; i++) body.vertical_vec[i] = dest[i];
		
    usbprint("%f, %f, %f\n", body.vertical_vec[1], body.vertical_vec[2], body.vertical_vec[0]);
    // usbprint("%ld,%ld,%ld,%ld,%d,%d,%d,%d,%d,%d,%d,%d\r\n", 
    // torque[1], torque[2], torque[3],
    // torque[4], torque[5], torque[6],
    // torque[7], torque[8], torque[9],
    // torque[10], torque[11], torque[12]
    // );

    pitch = 1.0f*euler[0]/65536.f;                   //ݫlong`эքMPLˤԶ˽ߝתۻԉއ׈եλ
    roll  = 1.0f*euler[1]/65536.f;
    yaw   = 1.0f*euler[2]/65536.f;
		HAL_UART_Receive_IT(&huart2, (uint8_t *)uart1Buf, 1);

		for(int i = RF_SERVO_1; i <= LB_SERVO_3; i++)
			torque[i] = ReadTorque(i);
    // usbprint("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", 
    // torque[1], torque[2], torque[3],
    // torque[4], torque[5], torque[6],
    // torque[7], torque[8], torque[9],
    // torque[10], torque[11], torque[12]
    // );
//    usbprint("pitch:%lf\t\troll:%lf\t\tyaw:%lf\t\tt:%ld\t\t\r\n", pitch, roll, yaw, HAL_GetTick());

    osDelay(5);
  }
  /* USER CODE END MPU_Task */
}

